#include "cloudprocesswidget.h"
#include "ui_cloudprocesswidget.h"
#include <QDebug>
#include <QDateTime>
#include <QMessageBox>

static int log_line_index = 0;

void TransformScreentPosToWorld(double* displayPos, double* world, pcl::visualization::PCLVisualizer::Ptr viewer)
{
    vtkRenderer* renderer{viewer->getRendererCollection()->GetFirstRenderer()};
    // First compute the equivalent of this display point on the focal plane
    double fp[4], tmp1[4],  eventFPpos[4];
    fp[3] = 0.0;
    renderer->SetWorldPoint(fp);
    renderer->WorldToDisplay();
    renderer->GetDisplayPoint(tmp1);

    tmp1[0] = displayPos[0];
    tmp1[1] = displayPos[1];

    renderer->SetDisplayPoint(tmp1);
    renderer->DisplayToWorld();

    renderer->GetWorldPoint(eventFPpos);
    // Copy the result
    for (int i=0; i<3; i++)
    {
        world[i] = eventFPpos[i];
    }
}

void pp_callback(const pcl::visualization::PointPickingEvent& event, void* args)
{
    struct callback_args* data = (struct callback_args *)args;
    if(event.getPointIndex() == -1) return;

    pcl::PointXYZ current_point;
    event.getPoint(current_point.x, current_point.y, current_point.z);

//    double displayPos[3] = {current_point.x, current_point.y, current_point.z};
//    double world[3];
//    TransformScreentPosToWorld(displayPos, world, data->viewer);

//    current_point.x = world[0];
//    current_point.y = world[1];
//    current_point.z = world[2];

    data->clicked_points->clear();
    data->clicked_points->points.push_back(current_point);

    pcl::visualization::PointCloudColorHandlerCustom<PointT> red(data->clicked_points, 255, 0, 0);
    data->viewer->removePointCloud("clicked_points");
    data->viewer->addPointCloud(data->clicked_points, red, "clicked_points");
    data->viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "clicked_points");

    qDebug() << current_point.x << " " << current_point.y << " " << current_point.z;
}

CloudProcessWidget::CloudProcessWidget(QWidget *parent) :
    QWidget(parent),
    toolbar(new QToolBar),
    cloud_process_thread(new QThread),
    cloudprocess(new CloudProcessDepend),
    clicked_points(new pcl::PointCloud<pcl::PointXYZ>()),
    cb_args(new callback_args),
    ui(new Ui::CloudProcessWidget)
{
    ui->setupUi(this);

    Init_UI();
    Init_Signals();

    // 点云初始化
    cloud.reset(new PointCloudT);
    poly_mesh.reset(new pcl::PolygonMesh);

    //可视化对象初始化
    viewer.reset(new pcl::visualization::PCLVisualizer("viewer", false));

    //设置VTK可视化窗口指针
    ui->qvtkWidget->SetRenderWindow(viewer->getRenderWindow());

    // 设置窗口交互，窗口可接受键盘等事件
    viewer->setupInteractor(ui->qvtkWidget->GetInteractor(), ui->qvtkWidget->GetRenderWindow());

    // 设置背景颜色
    viewer->setBackgroundColor(0.655, 0.655, 0.655);

    connect(cloudprocess,&CloudProcessDepend::Signal_SendMessage,this,&CloudProcessWidget::on_Signal_Message);
    cloudprocess->moveToThread(this->cloud_process_thread);
    cloud_process_thread->start();

    cb_args->clicked_points = clicked_points;
    cb_args->viewer = this->viewer;
    viewer->registerPointPickingCallback(&pp_callback, (void*)cb_args);

    //ui->qvtkWidget->SetRenderWindow(viewer->getRenderWindow());
    viewer->setupInteractor(ui->qvtkWidget->GetInteractor(), ui->qvtkWidget->GetRenderWindow());
    viewer->resetCamera();
    ui->qvtkWidget->SetRenderWindow(viewer->getRenderWindow()); // 设置PCLViewer可视化器渲染窗口
    ui->qvtkWidget->update();

    ShowLog(QString("Ready"));
}

CloudProcessWidget::~CloudProcessWidget()
{
    delete(cloudprocess);
    delete(cloud_process_thread);
    cloudprocess = nullptr;
    cloud_process_thread = nullptr;

    delete ui;
}

///初始化UI界面
void CloudProcessWidget::Init_UI()
{
    ///添加工具栏
    ui->horizontalLayout_2->addWidget(toolbar);

    const QIcon action_frontIcon = QIcon(":/image/icon/front.png");
    action_front = new QAction(action_frontIcon,QStringLiteral("front"),this);
    action_front->setStatusTip(QStringLiteral("front"));
    toolbar->addAction(action_front);

    const QIcon action_backIcon = QIcon(":/image/icon/back.png");
    action_back = new QAction(action_backIcon,QStringLiteral("back"),this);
    action_back->setStatusTip(QStringLiteral("back"));
    toolbar->addAction(action_back);

    const QIcon action_leftIcon = QIcon(":/image/icon/left.png");
    action_left = new QAction(action_leftIcon,QStringLiteral("left"),this);
    action_left->setStatusTip(QStringLiteral("left"));
    toolbar->addAction(action_left);

    const QIcon action_rightIcon = QIcon(":/image/icon/right.png");
    action_right = new QAction(action_rightIcon,QStringLiteral("right"),this);
    action_right->setStatusTip(QStringLiteral("right"));
    toolbar->addAction(action_right);

    const QIcon action_upIcon = QIcon(":/image/icon/up.png");
    action_up = new QAction(action_upIcon,QStringLiteral("up"),this);
    action_up->setStatusTip(QStringLiteral("up"));
    toolbar->addAction(action_up);

    const QIcon action_bottomIcon = QIcon(":/image/icon/bottom.png");
    action_bottom = new QAction(action_bottomIcon,QStringLiteral("bottom"),this);
    action_bottom->setStatusTip(QStringLiteral("bottom"));
    toolbar->addAction(action_bottom);

    const QIcon action_frontIsoIcon = QIcon(":/image/icon/frontIso.png");
    action_frontIso = new QAction(action_frontIsoIcon,QStringLiteral("frontIso"),this);
    action_frontIso->setStatusTip(QStringLiteral("up"));
    toolbar->addAction(action_frontIso);

    const QIcon action_backIsoIcon = QIcon(":/image/icon/backIso.png");
    action_backIso = new QAction(action_backIsoIcon,QStringLiteral("backIso"),this);
    action_backIso->setStatusTip(QStringLiteral("backIso"));
    toolbar->addAction(action_backIso);

    toolbar->setOrientation(Qt::Vertical);
}

void CloudProcessWidget::Init_Signals()
{
    // 绑定信号与槽
    connect(action_front, &QAction::triggered, this, &CloudProcessWidget::on_action_front_triggered);
    connect(action_left, &QAction::triggered, this, &CloudProcessWidget::on_action_left_triggered);
    connect(action_right, &QAction::triggered, this, &CloudProcessWidget::on_action_right_triggered);
    connect(action_up, &QAction::triggered, this, &CloudProcessWidget::on_action_up_triggered);
    connect(action_back, &QAction::triggered, this, &CloudProcessWidget::on_action_back_triggered);
    connect(action_bottom, &QAction::triggered, this, &CloudProcessWidget::on_action_button_triggered);
    connect(action_frontIso, &QAction::triggered, this, &CloudProcessWidget::on_actionFront_triggered);
    connect(action_backIso, &QAction::triggered, this, &CloudProcessWidget::on_actionBack_triggered);
}

/** 窗口打印日志信息 **/
void CloudProcessWidget::ShowLog(QString &info)
{
    QDateTime dateTime= QDateTime::currentDateTime();//获取系统当前的时间
    QString datetime_str = dateTime.toString("hh:mm:ss");//格式化时间
    log_line_index++;
    QString str = QString("<span style=\"color: green;\">%1 [%2]</span> %3").arg(datetime_str).arg(log_line_index).arg(info);
    ui->textEdit_showLog->append(str);
}

void CloudProcessWidget::ShowErrorLog(QString &info)
{
    QDateTime dateTime= QDateTime::currentDateTime();//获取系统当前的时间
    QString datetime_str = dateTime .toString("hh:mm:ss");//格式化时间
    log_line_index++;
    QString str = QString("<span style=\"color: red;\">%1 [%2]</span> %3").arg(datetime_str).arg(log_line_index).arg(info);
    ui->textEdit_showLog->append(str);
}

///分隔字符
void CloudProcessWidget::split(std::string &string_input, std::vector<std::string> &string_output, std::string &delema1)
{
    std::string::size_type start = string_input.find_first_not_of(delema1, 0);//找到第一个不为delema1的下标
    std::string::size_type pose = string_input.find_first_of(delema1, start);//找到第一个delema1的下标
    while (std::string::npos != start || std::string::npos != pose) {//当即没有delema1也没有字符的时候结束
        string_output.push_back(string_input.substr(start, pose - start));
        start = string_input.find_first_not_of(delema1, pose);//更新start 从pose开始
        pose = string_input.find_first_of(delema1, start);//更新pose,从start开始再次寻找
    }
}

double CloudProcessWidget::getMaxValue(PointType p1, PointType p2)
{
    double max = 0;

    if (p1.x - p2.x > p1.y - p2.y)
    {
        max = p1.x - p2.x;
    }
    else
    {
        max = p1.y - p2.y;
    }

    if (max < p1.z - p2.z)
    {
        max = p1.z - p2.z;
    }

    return max;
}

double CloudProcessWidget::getMinValue(PointType p1, PointType p2)
{
    double min = 0;

    if (p1.x - p2.x > p1.y - p2.y)
    {
        min = p1.y - p2.y;
    }
    else
    {
        min = p1.x - p2.x;
    }

    if (min > p1.z - p2.z)
    {
        min = p1.z - p2.z;
    }

    return min;
}

/** 显示NURBS曲线 **/
void CloudProcessWidget::ShowNURBSCurve(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, double r, double g, double b, bool IFSHOWPOINT = false)
{
    for (std::size_t i = 0; i < cloud->size() - 1; i++)	//画线
    {
        pcl::PointXYZ& p1 = cloud->at(i);
        pcl::PointXYZ& p2 = cloud->at(i + 1);
        std::ostringstream os;
        os << "line_" << r << "_" << g << "_" << b << "_" << i;
        viewer->addLine<pcl::PointXYZ>(p1, p2, r, g, b, os.str());
    }

    if (IFSHOWPOINT) {	//点
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler(cloud, 255 * r, 255 * g, 255 * b);
        viewer->addPointCloud<pcl::PointXYZ>(cloud, handler, "cloud_NURBS_Points");

        // 设置点大小
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1.0, "cloud_NURBS_Points");
    }
}

void CloudProcessWidget::ShowNURBSCurve(pcl::PointCloud<pcl::PointXYZ> cloud, double r, double g, double b, bool IFSHOWPOINT, std::string namestr)
{
    for (std::size_t i = 0; i < cloud.size() - 1; i++)	//画线
    {
        pcl::PointXYZ& p1 = cloud.at(i);
        pcl::PointXYZ& p2 = cloud.at(i + 1);
        std::ostringstream os;
        os << "line_" << r << "_" << g << "_" << b << "_" << i;
        viewer->addLine<pcl::PointXYZ>(p1, p2, r, g, b, namestr + os.str());
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr show_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::copyPointCloud(cloud, *show_cloud);
    if (IFSHOWPOINT) {	//点
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler(show_cloud, 255 * r, 255 * g, 255 * b);
        viewer->addPointCloud<pcl::PointXYZ>(show_cloud, handler, namestr + "cloud_NURBS_Points");

        // 设置点大小
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1.0, namestr + "cloud_NURBS_Points");
    }
}

/** 显示NURBS曲线上点和法线 **/
void CloudProcessWidget::ShowNURBSNormals(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::Normal>::Ptr& NURBS_normals)
{
    // 设置点云颜色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);
    viewer->addPointCloud(cloud, single_color, "NURBS Cloud");

    //设置点大小
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3.2, "NURBS Cloud");

    // 设置法线长度
    float line_length = 12.0; // 法线长度，根据点云规模调整

    // 添加法线到可视化窗口
    viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, NURBS_normals, 1, line_length, "NURBS normals");

    // 设置法线颜色
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "NURBS normals");
}

void CloudProcessWidget::ShowNURBSNormals(pcl::PointCloud<pcl::PointXYZ> cloud, pcl::PointCloud<pcl::Normal> NURBS_normals, std::string namestr, float scale = 10.0)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr show_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::Normal>::Ptr show_NURBS_normals(new pcl::PointCloud<pcl::Normal>());
    pcl::copyPointCloud(cloud, *show_cloud);
    pcl::copyPointCloud(NURBS_normals,*show_NURBS_normals);

    // 设置点云颜色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(show_cloud, 255, 0, 0);
    viewer->addPointCloud(show_cloud, single_color, namestr + "NURBS Cloud");

    //设置点大小
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3.2, namestr + "NURBS Cloud");

    for(size_t i=0; i<NURBS_normals.size(); ++i){
        pcl::PointXYZ point_end;
        point_end.x = cloud[i].x + NURBS_normals[i].normal_x * scale;
        point_end.y = cloud[i].y + NURBS_normals[i].normal_y * scale;
        point_end.z = cloud[i].z + NURBS_normals[i].normal_z * scale;
        std::ostringstream os;
        os << namestr << "_" << i << "_" << "line Normal direction";
        viewer->addLine<pcl::PointXYZ>(cloud[i], point_end, 0, 0, 1, os.str());
    }
}

/** 显示NURBS曲线点位姿 **/
void CloudProcessWidget::ShowNURBSVectorDirections(pcl::PointCloud<pcl::PointXYZ> cloud,
    std::vector<CloudProcessDepend::Pose_Vector> NURBS_direction_vectors, std::string id, float scale = 1.2)
{
    for (size_t i = 0; i < cloud.size(); i++)
    {
        //X方向
        pcl::PointXYZ point_end;
        point_end.x = cloud[i].x + NURBS_direction_vectors[i].X_Vector[0] * scale;
        point_end.y = cloud[i].y + NURBS_direction_vectors[i].X_Vector[1] * scale;
        point_end.z = cloud[i].z + NURBS_direction_vectors[i].X_Vector[2] * scale;
        std::ostringstream os;
        os << id << "_" << i << "_" << "line X direction";
        viewer->addLine<pcl::PointXYZ>(cloud[i], point_end, 1, 0, 0, os.str());

        //Y方向
        point_end.x = cloud[i].x + NURBS_direction_vectors[i].Y_Vector[0] * scale;
        point_end.y = cloud[i].y + NURBS_direction_vectors[i].Y_Vector[1] * scale;
        point_end.z = cloud[i].z + NURBS_direction_vectors[i].Y_Vector[2] * scale;
        os << id << "_" << i << "_" << "line Y direction";
        viewer->addLine<pcl::PointXYZ>(cloud[i], point_end, 0, 1, 0, os.str());

        //Z方向
        point_end.x = cloud[i].x + NURBS_direction_vectors[i].Z_Vector[0] * scale;
        point_end.y = cloud[i].y + NURBS_direction_vectors[i].Z_Vector[1] * scale;
        point_end.z = cloud[i].z + NURBS_direction_vectors[i].Z_Vector[2] * scale;
        os << id << "_" << i << "_" << "line Z direction";
        viewer->addLine<pcl::PointXYZ>(cloud[i], point_end, 0, 0, 1, os.str());
    }
}

void CloudProcessWidget::on_action_front_triggered()
{
    if (!cloud->empty())
    {
        pcl::getMinMax3D(*cloud, p_min, p_max);		// 获得最小最大值点xyz坐标
        maxLen = getMaxValue(p_max, p_min);
        viewer->setCameraPosition(0.5*(p_min.x + p_max.x), p_max.y + 2 * maxLen, 0.5*(p_min.z + p_max.z), 0.5*(p_min.x + p_max.x), 0, 0.5*(p_min.z + p_max.z), 0, 0, 1);

        ui->qvtkWidget->update();
    }
}

void CloudProcessWidget::on_action_up_triggered()
{
    if (!cloud->empty())
    {
        pcl::getMinMax3D(*cloud, p_min, p_max);		// 获得最小最大值点xyz坐标
        maxLen = getMaxValue(p_max, p_min);
        viewer->setCameraPosition(0.5*(p_min.x + p_max.x), 0.5*(p_min.y + p_max.y), p_max.z + 2 * maxLen, 0.5*(p_min.x + p_max.x), 0.5*(p_min.y + p_max.y), 0, 0, 1, 0);
        ui->qvtkWidget->update();
    }
}

void CloudProcessWidget::on_action_button_triggered()
{
    if (!cloud->empty())
    {
        pcl::getMinMax3D(*cloud, p_min, p_max);		// 获得最小最大值点xyz坐标
        maxLen = getMaxValue(p_max, p_min);
        viewer->setCameraPosition(0.5*(p_min.x + p_max.x), 0.5*(p_min.y + p_max.y), p_min.z - 2 * maxLen, 0.5*(p_min.x + p_max.x), 0.5*(p_min.y + p_max.y), 0, 0, 1, 0);
        ui->qvtkWidget->update();
    }
}

void CloudProcessWidget::on_action_left_triggered()
{
    if (!cloud->empty())
    {
        pcl::getMinMax3D(*cloud, p_min, p_max);		// 获得最小最大值点xyz坐标
        maxLen = getMaxValue(p_max, p_min);
        viewer->setCameraPosition(p_min.x - 2 * maxLen, 0.5*(p_min.y + p_max.y), 0.5*(p_min.z + p_max.z), 0, 0.5*(p_min.y + p_max.y), 0.5*(p_min.z + p_max.z), 0, 0, 1, 0);
        ui->qvtkWidget->update();
    }
}

void CloudProcessWidget::on_action_right_triggered()
{
    if (!cloud->empty())
    {
        pcl::getMinMax3D(*cloud, p_min, p_max);		// 获得最小最大值点xyz坐标
        maxLen = getMaxValue(p_max, p_min);
        viewer->setCameraPosition(p_max.x + 2 * maxLen, 0.5*(p_min.y + p_max.y), 0.5*(p_min.z + p_max.z), 0, 0.5*(p_min.y + p_max.y), 0.5*(p_min.z + p_max.z), 0, 0, 1, 0);
        ui->qvtkWidget->update();
    }
}

void CloudProcessWidget::on_action_back_triggered()
{
    if (!cloud->empty())
    {
        pcl::getMinMax3D(*cloud, p_min, p_max);		// 获得最小最大值点xyz坐标
        maxLen = getMaxValue(p_max, p_min);
        viewer->setCameraPosition(0.5*(p_min.x + p_max.x), p_min.y - 2 * maxLen, 0.5*(p_min.z + p_max.z), 0.5*(p_min.x + p_max.x), 0, 0.5*(p_min.z + p_max.z), 0, 0, 1);
        ui->qvtkWidget->update();
    }
}

void CloudProcessWidget::on_actionFront_triggered()
{
    if (!cloud->empty())
    {
        pcl::getMinMax3D(*cloud, p_min, p_max);		// 获得最小最大值点xyz坐标
        maxLen = getMaxValue(p_max, p_min);
        viewer->setCameraPosition(p_min.x - 2 * maxLen, p_min.y - 2 * maxLen, p_max.z + 2 * maxLen, 0.5*(p_min.x + p_max.x), 0.5*(p_min.y + p_max.y), 0.5*(p_min.z + p_max.z), 1, 1, 0);
        ui->qvtkWidget->update();
    }
}

void CloudProcessWidget::on_actionBack_triggered()
{
    if (!cloud->empty())
    {
        pcl::getMinMax3D(*cloud, p_min, p_max);		// 获得最小最大值点xyz坐标
        maxLen = getMaxValue(p_max, p_min);
        viewer->setCameraPosition(p_max.x + 2 * maxLen, p_max.y + 2 * maxLen, p_max.z + 2 * maxLen, 0.5*(p_min.x + p_max.x), 0.5*(p_min.y + p_max.y), 0.5*(p_min.z + p_max.z), -1, -1, 0);
        ui->qvtkWidget->update();
    }
}

void CloudProcessWidget::on_Signal_Message(QString message)
{
    ShowLog(message);
}


///导入点云文件
void CloudProcessWidget::on_pushButton_addCloud_clicked()
{
    // 获取文件路径
    QString path = QFileDialog::getOpenFileName(this, "选择点云文件", ".//", "点云文件(*.txt *.pcd);;所有文件(*.*)");

    // 识别文件类型
    QMimeDatabase db;
    QMimeType file_type = db.mimeTypeForFile(path);
    ShowLog(QString("导入点云文件"));

    if (file_type.name().startsWith("text/"))	// 文件为"txt"格式
    {
        std::ifstream inf;

        try
        {
            inf.open(path.toStdString());
        }
        catch (const std::exception&)	// 文件打开异常
        {
            ShowErrorLog(QString("文件打开异常"));
            return;
        }
        std::string sline;//每一行
        std::vector<std::string>string_output;
        std::string Delema = " ";
        pcl::PointXYZ point3d;

        cloud->clear();
        while (getline(inf, sline))
        {
            int i = 0;
            split(sline, string_output, Delema);		// 通过Delema字符提取分隔坐标
            point3d.x = stold(string_output[i]);		// 字符串转为long double ,并赋值
            point3d.y = stold(string_output[i + 1]);
            point3d.z = stold(string_output[i + 2]);
            string_output.clear();
            cloud->push_back(point3d);
        }
        inf.close();

        viewer->removeAllPointClouds();
        viewer->removeAllShapes();
        viewer->removeAllCoordinateSystems();

        // 点云设置颜色
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 0, 0); // 自定义颜色处理器
        viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "cloud sample");

        // 计算点云的边界盒
        Eigen::Vector4f minPt, maxPt;
        pcl::getMinMax3D(*cloud, minPt, maxPt);

        // 计算边界盒的尺寸
        float xSize = maxPt[0] - minPt[0];
        float ySize = maxPt[1] - minPt[1];
        float zSize = maxPt[2] - minPt[2];

        // 确定坐标系的缩放因子，这里取边界盒尺寸的最大值
        float scaleFactor = std::max(std::max(xSize, ySize), zSize) * 0.1; // 乘以一个系数以适应大小

        // 设置点云尺寸
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud sample");

        // 设置坐标系
        Eigen::Vector4f centroid(0,0,0,0);
        viewer->addCoordinateSystem(scaleFactor, centroid(0), centroid(1), centroid(2), 0);

        viewer->updatePointCloud(cloud, "cloud");
        viewer->resetCamera();
        ui->qvtkWidget->update();
    }
    else  // 文件为"pcd"格式
    {
        if (!path.isEmpty())
        {
            std::string path_name = path.toStdString();
            pcl::PCLPointCloud2 cloud2;
            Eigen::Vector4f origin;
            Eigen::Quaternionf orientation;
            int pcd_version;
            int data_type;
            unsigned int data_idx;
            pcl::PCDReader rd;
            rd.readHeader(path_name, cloud2, origin, orientation, pcd_version, data_type, data_idx);

            cloud->clear();
            if (data_type == 0)
            {
                if(pcl::io::loadPCDFile(path_name, *cloud) == -1){
                    return;
                }
            }
            else if (data_type == 2)
            {
                pcl::PCDReader reader;
                reader.read<pcl::PointXYZ>(path_name, *cloud);
            }
        }

        viewer->removeAllPointClouds();
        viewer->removeAllShapes();
        viewer->removeAllCoordinateSystems();

        // 点云设置颜色
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 0, 0); // 自定义颜色处理器
        viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "cloud");

        // 计算点云的边界盒
        Eigen::Vector4f minPt, maxPt;
        pcl::getMinMax3D(*cloud, minPt, maxPt);

        // 计算边界盒的尺寸
        float xSize = maxPt[0] - minPt[0];
        float ySize = maxPt[1] - minPt[1];
        float zSize = maxPt[2] - minPt[2];

        // 确定坐标系的缩放因子，这里取边界盒尺寸的最大值
        float scaleFactor = std::max(std::max(xSize, ySize), zSize) * 0.1; // 乘以一个系数以适应大小

        // 设置点云尺寸
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");

        Eigen::Vector3f center(0, 0, 0);

        // (场景中心点的坐标)
        viewer->addCoordinateSystem(scaleFactor, center(0), center(1), center(2), 0);
        viewer->resetCamera();
        ui->qvtkWidget->update();
    }
    //viewer->setCameraPosition(0.0, 0.0, -1.0, 0.0, 1.0, 0.0);
}

/** 提取坐标 **/
void CloudProcessWidget::on_pushButton_extractCoordinates_clicked()
{
    if(this->cloud->empty()){
        return;
    }

    //提取算法
    extract_slices_NURBS.clear();           //切片点云
    extract_slices_points.clear();          //提取点坐标
    extrace_slices_NURBS_normals.clear();   //提取点坐标法向量
    extrace_slices_NURBS_direction_vectors.clear(); //位姿

    cloudprocess->setCuttingNUM(ui->spinBox_CuttingNUM->value());
    cloudprocess->setCuttingDistance(ui->doubleSpinBox_CuttingDistance->value());
    cloudprocess->setProjectThreshold(ui->doubleSpinBox_ProjectThreshold->value());
    cloudprocess->setSortDirection(ui->comboBox_setSortDirection->currentIndex());
    cloudprocess->setExtractPointsNUM(ui->spinBox_extracePointsNUM->value());

    if(ui->comboBox_cuttingWay->currentIndex() == 0)
    {
        ShowLog(QString("Extract..."));
        cloudprocess->InputCloud(*this->cloud);
        cloudprocess->CuttingCloudPlanarSlices(extract_slices_NURBS, extract_slices_points, extrace_slices_NURBS_normals, extrace_slices_NURBS_direction_vectors);
        ShowExtrectSlicesOutput();
    }
    else if(ui->comboBox_cuttingWay->currentIndex() == 1)
    {
        if(cb_args->clicked_points->size() != 0){
            ShowLog(QString("Extract..."));
            cloudprocess->InputCloud(*this->cloud);
            cloudprocess->CuttingCloudPlanarOnceSlice(extract_slices_NURBS, extract_slices_points,
                                                      extrace_slices_NURBS_normals, extrace_slices_NURBS_direction_vectors,clicked_points->back());
            ShowExtrectSlicesOutput();
        }
        else{
            ShowErrorLog(QString("Choose One Point"));
        }
    }
}

void CloudProcessWidget::ShowExtrectSlicesOutput()
{
    //显示曲线和提取点
    viewer->removeAllShapes();
    viewer->removeAllPointClouds();
    ShowLog(QString("显示NURBS曲线和提取点"));
    for (size_t i = 0; i < extract_slices_points.size(); i++)
    {
        std::ostringstream os;
        os << "NURBS_line_" << i;

        //显示NNURBS曲线
        ShowNURBSCurve(extract_slices_NURBS[i], 0, 1, 0, true, os.str());

        //显示提取点和法向量
        ShowNURBSNormals(extract_slices_points[i], extrace_slices_NURBS_normals[i], os.str(), 14.0);
    }

    ShowLog(QString("加载曲面文件"));
    if(LoadPlyFile("D://StudySpace//Robot//RobotSimulation//RobotSimulation//Cloud//hangkongyepian_small_02.ply"))
    {
        viewer->removePolygonMesh("poly_mesh");
        viewer->removePointCloud("cloud");
        viewer->addPolygonMesh(*poly_mesh,"poly_mesh");
    }

//    if(LoadPlyFile("D://StudySpace//Robot//RobotSimulation//RobotSimulation//Cloud//hangkongyepian.ply"))
//    {
//        viewer->removePolygonMesh("poly_mesh");
//        viewer->removePointCloud("cloud");
//        viewer->addPolygonMesh(*poly_mesh,"poly_mesh");
//    }

    ui->qvtkWidget->update();
}

/** 加载PLY格式文件 **/
bool CloudProcessWidget::LoadPlyFile(std::string filename)
{
    if(pcl::io::loadPLYFile(filename,*poly_mesh) == -1){
        qDebug() << "LoadPLYFile Error";
        ShowErrorLog(QString("LoadPLYFile Error"));
        return false;
    }
    return true;
}


void CloudProcessWidget::on_pushButton_extractCoordinates_2_clicked()
{

}

/** 显示提取点位姿 **/
void CloudProcessWidget::on_pushButton_extractPose_clicked()
{
    viewer->removeAllShapes();
    for (size_t i = 0; i < extract_slices_points.size(); i++)
    {
        std::ostringstream os;
        os << "NURBS_line_" << i;

        //显示提取点位姿
        ShowNURBSVectorDirections(extract_slices_points[i], extrace_slices_NURBS_direction_vectors[i], os.str(), 4.0);
    }
    ui->qvtkWidget->update();
}

/** save Pose **/
void CloudProcessWidget::on_pushButton_saveExtractPose_clicked()
{
    if(extract_slices_points.empty())
    {
        ShowErrorLog(QString("extract points are empty"));
        return;
    }

    QString folderPath = QFileDialog::getExistingDirectory(nullptr,tr("Select Directory"),
                                      QDir::homePath(),QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
    if(folderPath.isEmpty()) return;

    for(size_t i=0; i < extract_slices_points.size(); ++i){

        QString fileName = QString("TrajPlan_Position_") + QString::number(i+1) + QString(".txt");
        QString filePath = QDir(folderPath).filePath(fileName);

        QFile file(filePath);
        if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
            ShowErrorLog(QString("无法打开保存路径,保存目标点失败！"));
            return;
        }

        QTextStream out(&file);
        for (size_t j=0; j<extract_slices_points[i].size(); j++)
        {
            pcl::PointXYZ slices_point = extract_slices_points[i][j];
            CloudProcessDepend::Pose_Vector clices_Vector = extrace_slices_NURBS_direction_vectors[i][j];

            Eigen::Vector3f ux = clices_Vector.X_Vector; // x方向向量
            Eigen::Vector3f uy = clices_Vector.Y_Vector; // y方向向量
            Eigen::Vector3f uz = clices_Vector.Z_Vector; // z方向向量
            Eigen::Matrix3f R;
            R.col(0) = ux; // 设置旋转矩阵的第一列
            R.col(1) = uy; // 设置旋转矩阵的第二列
            R.col(2) = uz; // 设置旋转矩阵的第三列

            Eigen::Vector3f euler_angles = R.eulerAngles(0, 1, 2); // 按照XYZ顺序获取欧拉角

            double dX = slices_point.x; double dY = slices_point.y; double dZ = slices_point.z;
            double dRx = euler_angles[0]; double dRy = euler_angles[1]; double dRz = euler_angles[2];

            out << dX << ", " << dY << ", " << dZ << ", "
                << dRx << ", " << dRy << ", " << dRz << endl;
        }
        file.close();
    }

    ShowLog(QString("Save Extract Points Successful"));
}

/** set **/
void CloudProcessWidget::on_pushButton_Set_clicked()
{
    cloudprocess->setCuttingNUM(ui->spinBox_CuttingNUM->value());
    cloudprocess->setCuttingDistance(ui->doubleSpinBox_CuttingDistance->value());
    cloudprocess->setProjectThreshold(ui->doubleSpinBox_ProjectThreshold->value());
    cloudprocess->setSortDirection(ui->comboBox_setSortDirection->currentIndex());
    cloudprocess->setExtractPointsNUM(ui->spinBox_extracePointsNUM->value());
}

/** Recompute **/
void CloudProcessWidget::on_pushButton_ReCompute_clicked()
{
    viewer->removePolygonMesh("poly_mesh");
    viewer->removeAllPointClouds();
    viewer->removeAllShapes();

    if(!cloud->empty())
    {
        // 点云设置颜色
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 0, 0); // 自定义颜色处理器
        viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "cloud");

        // 设置点云尺寸
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");
    }
    ui->qvtkWidget->update();
}
